#include "ArmMngrNode/ArmMover.h"
#include <string>
#include "std_msgs/String.h"

#define PI 3.14159265
#define UPPER_ARM 35.0
#define LOWER_AND_MIDDLE_ARM 50.0
#define MIDDLE_ARM 25.0
#define MIDDLE_AND_UPPER_ARM 58.0
#define LOWER_ARM 27.0

#define ZERO_DIV_CORRECTION 0.00001526 // 1*2^-16 = shift left 16
#define NEG_ROTATE_CORRECTION 0.00001526 // 1*2^-16 = shift left 16

using namespace std;

ArmMover* arm_mover;

// ROS management objects
ros::Rate* loop_rate;

double getDistance(double x, double y);
void calcAngle(double cord_i, double cord_j, double a, double b, double* alpha,
		double* gamma);

void moveArm(double x, double y, double z);
void moveVertical(double x, double z);
void moveHorizontal(double x, double y);

double getShoulderMotorPosition(double alpha);
double getBaseRotationMotorPosition(double alpha);
double getElbowMotorPosition(double gamma);

void DemoArmListener(const std_msgs::String::ConstPtr& msg) {

	moveArm(47,-15,60);
	sleep(3);

	moveArm(47,15,60);
	sleep(3);

	moveArm(47,15,45);
	sleep(1);

	moveArm(47,-15,45);
	sleep(3);

	moveArm(47,-15,60);


	return;

	// const char* point = msg->data.c_str();
	// double x = (point[0] - '0') * 10.0 + (point[1] - '0') * 1.0;

	// double y = (point[3] - '0') * 10.0 + (point[4] - '0') * 1.0;

	// if ('-' == point[2]){

	// 	y *= -1;		
	// }

	// double z = (point[6] - '0') * 10.0 + (point[7] - '0') * 1.0;

	// if ('-' == point[5]){


	// 	moveHorizontal(x, y);
	// 	arm_mover->MoveUpperElbow(0);
				
	// 	z *= -1;	

	// 	sleep(2*y/15);
	// }


	// printf("msg = %s, x = %f, y = %f, z = %f\n", point, x, y, z);

	// moveHorizontal(x, y);
	// moveVertical(getDistance(x, y), z);
}

int main(int argc, char **argv) {

	// initialize system
	ros::init(argc, argv, "DemoNode");
	ros::NodeHandle node_handler;

	loop_rate = new ros::Rate(1);

	// allocating the rover controller
	arm_mover = new ArmMover();

	// Minimal (10000 not worked) time to sleep after advertising a topic
	// usleep(1000000);
	sleep(1);

	// subscribe to sensors module
	int queue_depth = 10;

	// check witch sensors are subscribed
	ros::Subscriber sub_sensors_chanel = node_handler.subscribe(
			"/DemoNode/MoveArm", queue_depth, DemoArmListener);

	// initialize arm joints
	arm_mover->MoveUpperElbow(0.5);
	arm_mover->MoveLowerElbow(0);
	arm_mover->MoveShoulder(0.5);
	arm_mover->MoveWrist(0);
	arm_mover->RotateBase(0);
	arm_mover->MoveLeftFinger(-0.5);
	arm_mover->MoveRightFinger(0.6);

	sleep(2);

	

	// keep listening to subscribers until ros::shutdown() has been called and is finished
	while (ros::ok()) {
		ros::spinOnce();
	}

	delete arm_mover;
	return 0;
}

void moveArm(double x, double y, double z) {

	moveHorizontal(x, y);
	// TODO explain getDistance(x, y)
	moveVertical(getDistance(x, y), z);
}

void moveVertical(double x, double z) {

	double alpha = 0;
	double gamma = 0;

	printf("in moveVertical. x = %f, z = %f\n", x, z);

	calcAngle(x, z, UPPER_ARM, LOWER_AND_MIDDLE_ARM, &alpha, &gamma);

	arm_mover->MoveShoulder(getShoulderMotorPosition(alpha));
	arm_mover->MoveUpperElbow(getElbowMotorPosition(gamma));
}

void moveHorizontal(double x, double y) {

	double rotation_angle = (atan(double(x) / double(y+ZERO_DIV_CORRECTION)) * 180.0 / PI);

	if (y < 0) {

		rotation_angle -= NEG_ROTATE_CORRECTION;
	}


	printf("rotation_angle = %f\n", rotation_angle);


	arm_mover->RotateBase(getBaseRotationMotorPosition(rotation_angle));
}

//triangle...
void calcAngle(double cord_i, double cord_j, double a, double b, double* alpha,
		double* gamma) {

	double transform_angle = (atan(double(cord_j) / double(cord_i+ZERO_DIV_CORRECTION)) * 180.0 / PI);
	double c = getDistance(cord_i, cord_j);

	*alpha = acos((b * b + c * c - a * a) / (2 * b * c)) * 180.0 / PI;
	*gamma = 180 - (*alpha)
			- acos((a * a + c * c - b * b) / (2 * a * c)) * 180.0 / PI;

	// gamma must use alpha before transformation
	*alpha += transform_angle;
}

double getDistance(double x, double y) {

	return sqrt(x * x + y * y);
}

double getShoulderMotorPosition(double alpha) {

	printf("Shoulder:\n");
	printf("%f\n", alpha);

	return 1.5 - (alpha) / 60;
}

double getBaseRotationMotorPosition(double alpha) {

	printf("BaseRotation:\n");
	printf("%f\n", alpha);

	if (alpha < 0) {

		return 1.5 - (180+alpha) / 60;
	}

	return 1.5 - (alpha) / 60;
}

double getElbowMotorPosition(double gamma) {

	printf("\nElbow:\n");
	printf("%f\n", gamma);

	return 1.5 + (90 - gamma) / 60;
}

// void checkAngles(int x,int y,int a, int b, int* shoulder_angle , int* upper_elbow_angle) {

// 	double alpha = 0.0;
// 	double gamma = 0.0;

// 	for (int shoulder_position = -180; shoulder_position <= 180; shoulder_position+=1) {

// 		for (int upper_elbow_position = -180; upper_elbow_position <= 180; upper_elbow_position+=1) {

// 			double theta = 360 - shoulder_position - upper_elbow_position - 90;
// 			double gamma_tag = 180 - theta;

// 			double w = abs(a*sin(gamma_tag*PI/180.0));
// 			double v = abs(b*cos(shoulder_position*PI/180.0));

// 			int new_x = v+w;
// 			int new_y = w;

// 			alpha = 0.0;
// 			gamma = 0.0;
// 			// Move horizontal first
// 			calcAngle(x, y, a, b, &alpha, &gamma);

// 				if (int(alpha)== shoulder_position) {
// 				if (int(gamma)== upper_elbow_position) {
// 			// if (new_x > 83 && new_x < 86) {
// 					printf("NEW_X = %d NEW_Y = %d\n", shoulder_position,upper_elbow_position);
// 			// 	if (new_y > -1 && new_y < 2) {

// 					*shoulder_angle = shoulder_position;
// 					*upper_elbow_angle = upper_elbow_position;

// 					shoulder_position = 181;
// 					// printf("NEW_X = %d NEW_Y = %d\n", new_x,new_y);

// 					break;
// 			}
// 		}

// 		}

// 	}

// }

// double get_A_Angle(int x, int y){

// 	double C = getDistanceToPoint(x,y);

// 	return acos((LOWER_ARM*LOWER_ARM + C*C - MIDDLE_AND_UPPER_ARM*MIDDLE_AND_UPPER_ARM)/(2*LOWER_ARM*C)) * 180.0 / PI;
// }

// double get_B_Angle(int x, int y){

// 	double C = getDistanceToPoint(x,y);

// 	return acos((MIDDLE_AND_UPPER_ARM*MIDDLE_AND_UPPER_ARM + C*C - LOWER_ARM*LOWER_ARM)/(2*MIDDLE_AND_UPPER_ARM*C)) * 180.0 / PI;
// }

// double get_C_Angle(int x, int y){

// 	return 180 - get_A_Angle(x,y) - get_B_Angle(x,y);

// }

// double alpha = 0.0;
// 	double gamma = 0.0;

// 	double real_lower_and_middle_arm_length = LOWER_AND_MIDDLE_ARM;
// 	double real_upper_and_middle_arm_length = MIDDLE_AND_UPPER_ARM;

// 	// Vertical first
// 	calcAngle(x, z, UPPER_ARM, real_lower_and_middle_arm_length, &alpha,
// 			&gamma);

// 	// get real distance of real_upper_and_middle_arm_length which changed after vertical move
// 	if (isnan(gamma)) {
// 		real_upper_and_middle_arm_length = MIDDLE_AND_UPPER_ARM;
// 	} else {

// 		arm_mover->MoveShoulder(getShoulderMotorPosition(alpha));
// 		arm_mover->MoveUpperElbow(getElbowMotorPosition(gamma));

// 		real_upper_and_middle_arm_length = sqrt(
// 				UPPER_ARM * UPPER_ARM + MIDDLE_ARM * MIDDLE_ARM
// 						- 2 * UPPER_ARM * MIDDLE_ARM * cos(gamma * PI / 180.0));
// 	}

// 	// Move horizontal
// 	calcAngle(x, y, real_upper_and_middle_arm_length, LOWER_ARM, &alpha,
// 			&gamma);

// 	// Horizontal
// 	// get real distance of real_upper_and_middle_arm_length which changed after vertical move
// 	if (!isnan(gamma)) {
// 		if (0 == y){
// 			arm_mover->RotateBase(0.0);
// 			arm_mover->MoveLowerElbow(0.0);
// 		} else {

// 			arm_mover->RotateBase(getBaseRotationMotorPosition(alpha));
// 			arm_mover->MoveLowerElbow(getElbowMotorPosition(gamma));
// 		}
// 	}

// 	sleep(1);

// 	for (int i = 0 ; i < 9 ; ++i) {

// 		printf("Iteration number: %d\n", i);

// 		// Vertical
// 		// get real distance of real_upper_and_middle_arm_length which changed after horizontal move
// 		if (isnan(gamma)) {
// 			real_lower_and_middle_arm_length = MIDDLE_AND_UPPER_ARM;
// 		} else {

// 			arm_mover->MoveShoulder(getShoulderMotorPosition(alpha));
// 			arm_mover->MoveUpperElbow(getElbowMotorPosition(gamma));

// 			real_lower_and_middle_arm_length = sqrt(
// 							LOWER_ARM * LOWER_ARM + MIDDLE_ARM * MIDDLE_ARM
// 									- 2 * LOWER_ARM * MIDDLE_ARM * cos(gamma * PI / 180.0));
// 		}

// 		calcAngle(x, z, UPPER_ARM, real_lower_and_middle_arm_length, &alpha,
// 				&gamma);

// 		// Horizontal
// 		// get real distance of real_upper_and_middle_arm_length which changed after horizontal move
// 		if (isnan(gamma)) {
// 			real_upper_and_middle_arm_length = MIDDLE_AND_UPPER_ARM;
// 		} else {

// 			if (!isnan(gamma)) {
// 				if (0 == y){
// 					arm_mover->RotateBase(0.0);
// 					arm_mover->MoveLowerElbow(0.0);
// 				} else {

// 					arm_mover->RotateBase(getBaseRotationMotorPosition(alpha));
// 					arm_mover->MoveLowerElbow(getElbowMotorPosition(gamma));
// 				}
// 			}

// 			real_upper_and_middle_arm_length = sqrt(
// 							UPPER_ARM * UPPER_ARM + MIDDLE_ARM * MIDDLE_ARM
// 									- 2 * UPPER_ARM * MIDDLE_ARM * cos(gamma * PI / 180.0));
// 		}

// 		// Move horizontal first
// 		calcAngle(x, y, real_upper_and_middle_arm_length, LOWER_ARM, &alpha,
// 				&gamma);

// void moveHorizontal(double x, double y) {

// 	double alpha = 0.0;
// 	double gamma = 0.0;

// 	calcAngle(x, y, MIDDLE_AND_UPPER_ARM, LOWER_ARM, &alpha, &gamma);

// 	arm_mover->RotateBase(getBaseRotationMotorPosition(alpha));
// 	arm_mover->MoveLowerElbow(getElbowMotorPosition(gamma));
// }
